#include "usart.h"

#define MAX_STRLEN 256 // this is the maximum string length of our string in characters
volatile static char USART2_received_string[MAX_STRLEN];
volatile static char USART3_received_string[MAX_STRLEN];
volatile static char USART_GPS_STRING[MAX_STRLEN];

void USART_puts(USART_TypeDef* USARTx, volatile char *s);
void set_gps_variables();



void init_usart(void)
{

	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	/* enable peripheral clock for USART2 */
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);


	/* GPIOA clock enable */
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
	GPIO_Init(GPIOD, &GPIO_InitStructure);


    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;//11 is recv (dout)
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;// 10 is din
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;//  GPIO_PuPd_NOPULL = 0x00,    GPIO_PuPd_UP     = 0x01,    GPIO_PuPd_DOWN   = 0x02
    GPIO_Init(GPIOB, &GPIO_InitStructure);

	GPIO_PinAFConfig(GPIOD, GPIO_PinSource5, GPIO_AF_USART2);
	GPIO_PinAFConfig(GPIOD, GPIO_PinSource6, GPIO_AF_USART2);

    GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_USART3);
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3);


	USART_InitStructure.USART_BaudRate = 9600*3;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init(USART2, &USART_InitStructure);

	USART_InitStructure.USART_BaudRate = 111111*3;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
	USART_Init(USART3, &USART_InitStructure);

	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);

	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;	// we want to configure the USART2 interrupts
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;// this sets the priority group of the USART1 interrupts
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;	// this sets the subpriority inside the group
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	// the USART2 interrupts are globally enabled
	NVIC_Init(&NVIC_InitStructure);	// the properties are passed to the NVIC_Init function which takes care of the low level stuff

	NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;	// we want to configure the USART3 interrupts
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;// this sets the priority group of the USART1 interrupts
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;	// this sets the subpriority inside the group
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	// the USART3 interrupts are globally enabled
	NVIC_Init(&NVIC_InitStructure);	// the properties are passed to the NVIC_Init function which takes care of the low level stuff

	USART_Cmd(USART2, ENABLE); // enable USART2
	USART_Cmd(USART3, ENABLE); // enable USART3


	//$PMTK301,1*2D<CR><LF>


	int i = 0;
	for(;i<256;i++)
	{
		USART2_received_string[i]=0;
		USART3_received_string[i]=0;
	}
	set_gps_variables();
}

void set_gps_variables()
{

	USART_puts(USART2, "$PMTK301,2*2E\r\n");
	USART_puts(USART2, "$PMTK220,200*2C\r\n");
	USART_puts(USART2, "$PMTK251,57600*2C\r\n");
	USART_InitTypeDef USART_InitStructure;
	USART_InitStructure.USART_BaudRate = 57600*3;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
	USART_Init(USART2, &USART_InitStructure);

}

void USART_puts(USART_TypeDef* USARTx, volatile char *s)
{

	while(*s)
	{
		// wait until data register is empty
		while( !(USARTx->SR & 0x00000040) );
		USART_SendData(USARTx, *s);
		*s++;
	}
}

void fudgebitrate()
{

	static int fudge = 111111*3;
	USART_InitTypeDef USART_InitStructure;

	USART_InitStructure.USART_BaudRate = fudge;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init(USART3, &USART_InitStructure);
	USART_Cmd(USART3, ENABLE);
	fudge = fudge+1;
	char str[32];
	get_dec_str (str, 32, fudge);
	str[0]='\n';
	USART_puts(USART3, str);
	if(fudge>1000000)
		fudge=1;

}

// this is the interrupt request handler (IRQ) for ALL USART2 interrupts
void USART2_IRQHandler(void)
{

	// check if the USART2 receive interrupt flag was set
	if( USART_GetITStatus(USART2, USART_IT_RXNE) )
	{
		static uint8_t cnt = 0; // this counter is used to determine the string length
		char t = USART2->DR; // the character from the USART2 data register is saved in t

		/* check if the received character is not the LF character (used to determine end of string)
		 * or the if the maximum string length has been been reached
		 */
		if( (t != '\n') && (cnt < MAX_STRLEN) )
		{
			USART2_received_string[cnt] = t;
			cnt++;
		}
		else
		{ // otherwise reset the character counter and print the received string
			cnt = 0;
			strcpy(USART_GPS_STRING,USART2_received_string);
			//set_gps_variables();
		}
	}
}

// this is the interrupt request handler (IRQ) for ALL USART3 interrupts
void USART3_IRQHandler(void)
{

	// check if the USART3 receive interrupt flag was set
	if( USART_GetITStatus(USART3, USART_IT_RXNE) )
	{
		static uint8_t cnt = 0; // this counter is used to determine the string length
		char t = USART3->DR; // the character from the USART3 data register is saved in t

		/* check if the received character is not the LF character (used to determine end of string)
		 * or the if the maximum string length has been been reached
		 */
		if( (t != '\n') && (cnt < MAX_STRLEN) )
		{
			USART3_received_string[cnt] = t;
			USART_SendData(USART3, &t);
			cnt++;
		}
		else
		{ // otherwise reset the character counter and print the received string
			USART_puts(USART3, USART3_received_string);
			cnt = 0;


			//set_gps_variables();
		}
	}
}

void get_dec_str (uint8_t* str, char len, int oval)
{
  uint8_t i;
  int val = oval;
  for(i=1; i<=len; i++)
  {
    str[len-i] = (uint8_t) ((val % 10UL) + '0');
    val/=10;
  }
  if(oval<0)
	  str[0]='-';
  str[i-1] = '\0';
}

void Transmit_Telemetry(struct FlightDataStruct* state)
{
	char StartPacket[16]="\rBeginTelemetry: ";
	char Data[256];
	char Delimiter[2]=", ";
	strcpy(state->gpsString,USART_GPS_STRING);
	USART_puts(USART3, StartPacket);
	get_dec_str(Data,8,state->current_Accel1_X);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Accel1_Y);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Accel1_Z);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Accel2_X);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Accel2_Y);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Accel2_Z);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Gyro1_X);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Gyro1_Y);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Gyro1_Z);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Mag1_X);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Mag1_Y);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	get_dec_str(Data,8,state->current_Mag1_Z);
	USART_puts(USART3, Data);
	USART_puts(USART3, Delimiter);
	USART_puts(USART3, state->gpsString);
	USART_puts(USART3, Delimiter);







}
